#include <stdio.h>
#include <string.h>
#include "c3ga.h"

int main(int argc, char *argv[]) {
  FILE *fp, *op;
  char *line;
  float x, y, z, dis, d1, d2, d3;
  int nbytes = 100;
  int num, i, j;
  normalizedPoint *points;
  int *nearest;
  mv p1, p2, p3;

  fp = fopen(argv[1], "r");
  op = fopen(argv[2], "w");

  if (fp == NULL) {
    fprintf(stderr, "Can't open input file %s!\n", argv[1]);
    exit(1);
  } else if (op == NULL) {
    fprintf(stderr, "Can't open output file %s!\n", argv[2]);
  }

  line = (char *) malloc(100);

  getline(&line, &nbytes, fp);
  printf("%s\n", line);
  getline(&line, &nbytes, fp);
  printf("%s\n", line);
  getline(&line, &nbytes, fp);
  printf("%s\n", line);

  sscanf(line, "element vertex %d", &num);
  printf("[Found %d points]\n", num);
  
  while (strstr(line, "end_header") == NULL) {
    getline(&line, &nbytes, fp);
  }

  points = (normalizedPoint *) malloc(num * sizeof *points);
  nearest = (int *) malloc(num * 3 * sizeof *nearest);

  printf("[Loading dataset from '%s'...]\n", argv[1]);
  for (i = 0; i < num; i++) {
    getline(&line, &nbytes, fp);
    sscanf(line, "%f %f %f %*s", &x, &y, &z);
    cgaPoint_float_float_float(points + i, x, y, z);
    // DEBUG
//    normalizedPoint_to_mv(&p1, points + i);
//    toString_mv(&p1, line, 100, "%f");
//    printf("%s\n", line);
    // END DEBUG
  }

  printf("[Finding nearest neighbours...]\n");

  for (i = 0; i < num; i++) {
    d1 = d2 = d3 = 10000.0;
    normalizedPoint_to_mv(&p1, points + i);
    for (j = 0; j < num; j++) {
      if (j == i)
        continue;
      normalizedPoint_to_mv(&p2, points + j);
      lc_mv_mv(&p3, &p2, &p1);
      dis = fabs(mv_scalar(&p3));
      if (dis < d1) {
        d1 = dis;
        *(nearest + (i * 3)) = j;
      }
      else if (dis < d2) {
        d2 = dis;
        *(nearest + (i * 3) + 1) = j;
      }
      else if (dis < d3) {
        d3 = dis;
        *(nearest + (i * 3) + 2) = j;
      }
    }
    fprintf(op, "%d %d %d %d\n", i, *(nearest + (i * 3)), *(nearest + (i * 3) + 1), *(nearest + (i * 3) + 2));
  }

  printf("[Done, written output to '%s']\n", argv[2]);

  fclose(op);
  fclose(fp);

//  while (getline(line, 100, fp))

}
